#include "contour_filter.h"
#include <vector>
#include <algorithm>
#include <cmath>

using namespace std;

Contour* median(Contour* contour, int window){

    Contour* result = new Contour(contour->img);

    list<Position>::iterator currentbegin;
    list<Position>::iterator currentend;
    list<Position>::iterator walker;

    vector<int> vecwindowx;
    vector<int> vecwindowy;


    int begincounter=(window/2);
    int endcounter=1;

    currentbegin=contour->points.begin();

    currentend=currentbegin;
    for(int i=0;i<window/2;i++)
        currentend++;

    for(int k=0; k<contour->getPixelLenght(); k++){

        /*Amostra a janela*/
        for(int c=0;c<(begincounter-1);c++){
            vecwindowx.push_back((*currentbegin).x);
            vecwindowy.push_back((*currentbegin).y);
        }

            vecwindowx.push_back((*currentbegin).x);
            vecwindowy.push_back((*currentbegin).y);

        for(walker=currentbegin;walker!=currentend;walker++){
            vecwindowx.push_back((*walker).x);
            vecwindowy.push_back((*walker).y);
        }

        for(int d=0;d<endcounter;d++){
            vecwindowx.push_back((*currentend).x);
            vecwindowy.push_back((*currentend).y);
        }

        begincounter--;

        if(begincounter<0)
            currentbegin++;

        currentend++;

        if(currentend==contour->points.end()){
            currentend--;
            endcounter++;
        }

        std::sort(vecwindowx.begin(),vecwindowx.end());
        std::sort(vecwindowy.begin(),vecwindowy.end());

        int x=*(vecwindowx.begin()+((window/2)+1));
        int y=*(vecwindowy.begin()+((window/2)+1));
        result->addPoint(x,y);

        vecwindowx.clear();
        vecwindowy.clear();

    }

    return result;

}

void swap_(int* one,int* two){
    int temp=*one;
    *one=*two;
    *two=temp;
}

Contour* linearinterpolation(Contour* contour){
    Contour* result = new Contour(contour->img);

    list<Position>::iterator currentbegin;
    list<Position>::iterator currentend;

    currentbegin=contour->points.begin();

    currentend=currentbegin;
    currentend++;

    while(currentend!=contour->points.end()){

        int x0,x1,y0,y1;
        x0=(*currentbegin).x;
        y0=(*currentbegin).y ;
        x1=(*currentend).x;
        y1=(*currentend).y;

        if( (abs( x0 - x1 )>1) || (abs( y0 - y1 )>1) ){

               //from wikipedia
                int Dx = x1 - x0;
                int Dy = y1 - y0;
                int steep = (abs(Dy) >= abs(Dx));
                if (steep) {
                    swap_(&x0, &y0);
                    swap_(&x1, &y1);
                    // recompute Dx, Dy after swap
                    Dx = x1 - x0;
                    Dy = y1 - y0;
                }
                int xstep = 1;
                if (Dx < 0) {
                    xstep = -1;
                    Dx = -Dx;
                }
                int ystep = 1;
                if (Dy < 0) {
                    ystep = -1;
                    Dy = -Dy;
                }
                int TwoDy = 2*Dy;
                int TwoDyTwoDx = TwoDy - 2*Dx; // 2*Dy - 2*Dx
                int E = TwoDy - Dx; //2*Dy - Dx
                int y = y0;
                int xDraw, yDraw;
                for (int x = x0; x != x1; x += xstep) {
                    if (steep) {
                        xDraw = y;
                        yDraw = x;
                    } else {
                        xDraw = x;
                        yDraw = y;
                    }
                    // plot
                    result->addPoint(Position(xDraw,yDraw));
                    // next
                    if (E > 0) {
                        E += TwoDyTwoDx; //E += 2*Dy - 2*Dx;
                        y = y + ystep;
                    } else {
                        E += TwoDy; //E += 2*Dy;
                    }
                }
        }
        else{
            result->addPoint(Position(x0,y0));
        }

        currentbegin++;
        currentend++;
    }

    return result;
}
